'''blank'''
import math
from package.missleTest.q_case import CASE, getCases
from math import *
import unittest

# 地球半径为6371km
ER = 6371.393

'''Q1: 已知A,B两点经纬度，求AB距离'''
def jw2Rad(lng1, lat1, lng2, lat2):
    lng1, lat1, lng2, lat2 = map(radians, [float(lng1), float(lat1), float(lng2), float(lat2)])  # 经纬度转换成弧度
    return lng1, lat1, lng2, lat2

#<https://www.cnblogs.com/andylhc/p/9481636.html>  这个算得比较准，可以用<https://www.hhlink.com/%E7%BB%8F%E7%BA%AC%E5%BA%A6>进行验证
def geodistance(lng1, lat1, lng2, lat2):
    # lng1,lat1,lng2,lat2 = (120.12802999999997,30.28708,115.86572000000001,28.7427)
    # lng1, lat1, lng2, lat2 = map(radians, [float(lng1), float(lat1), float(lng2), float(lat2)]) # 经纬度转换成弧度
    lng1, lat1, lng2, lat2 = jw2Rad(lng1, lat1, lng2, lat2)
    dlon = lng2 - lng1
    dlat = lat2 - lat1
    a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2
    distance = 2 * asin(sqrt(a)) * 6371 # 地球平均半径，6371km
    return distance

'''Q2: 已知A,B两点经纬度，求B点相对于A的航向角'''
def rad(d):
    return d * pi / 180.0

def deg(x):
    return x * 180 / pi

#https://blog.csdn.net/sunqiande88/article/details/102501980
def getDegree(latA, lonA, latB, lonB):
    """
    Args:
        point p1(latA, lonA)
        point p2(latB, lonB)
    Returns:
        bearing between the two GPS points,
        default: the basis of heading direction is north
    """
    radLatA = radians(latA)
    radLonA = radians(lonA)
    radLatB = radians(latB)
    radLonB = radians(lonB)
    dLon = radLonB - radLonA
    y = sin(dLon) * cos(radLatB)
    x = cos(radLatA) * sin(radLatB) - sin(radLatA) * cos(radLatB) * cos(dLon)
    brng = degrees(atan2(y, x))
    brng = (brng + 360) % 360
    return brng

'''Q3: 已知A点经纬度，和A点到B点的距离和航向角，求B点经纬度'''
def getLonAndLat(lat, lng, brng, dist):
    a = 6378137
    b = 6356752.3142
    f = 1.0 / 298.257223563

    lon1 = lng * 1
    lat1 = lat * 1
    s = dist
    alpha1 = rad(brng)
    sinAlpha1 = sin(alpha1)
    cosAlpha1 = cos(alpha1)

    tanU1 = (1 - f) * tan(rad(lat1))
    cosU1 = 1 / sqrt((1 + tanU1 * tanU1))
    sinU1 = tanU1 * cosU1
    sigma1 = atan2(tanU1, cosAlpha1)
    sinAlpha = cosU1 * sinAlpha1
    cosSqAlpha = 1 - sinAlpha * sinAlpha
    uSq = cosSqAlpha * (a * a - b * b) / (b * b)
    A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))

    sigma = s / (b * A)
    sigmaP = 2 * pi
    while abs(sigma - sigmaP) > 1e-12:
        cos2SigmaM = cos(2 * sigma1 + sigma)
        sinSigma = sin(sigma)
        cosSigma = cos(sigma)
        deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                                                           B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (
                                                                   -3 + 4 * cos2SigmaM * cos2SigmaM)))
        sigmaP = sigma
        sigma = s / (b * A) + deltaSigma

    tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1
    lat2 = atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                 (1 - f) * sqrt(sinAlpha * sinAlpha + tmp * tmp))
    lambdA = atan2(sinSigma * sinAlpha1, cosU1*cosSigma - sinU1 * sinSigma * cosAlpha1)
    C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
    L = lambdA - (1-C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM+C * cosSigma * (-1+2 * cos2SigmaM * cos2SigmaM)))
    revAz = atan2(sinAlpha, -tmp)

    lon,lat = 180 + deg(lat2), lon1 + deg(L)
    return lat,lon

if __name__ == '__main__':

    cases = getCases()
    for index, case in enumerate(cases):
        print(f"#################     case{index}     ###################")
        A_w, A_j, B_w, B_j, L, bearing = case.A_w, case.A_j, case.B_w, case.B_j, case.L, case.bearing

        '''Ans: 已知A,B两点经纬度，求AB距离，航向角'''
        res_L = geodistance(A_j, A_w, B_j, B_w)
        res_bearing  = getDegree(A_w,A_j,B_w,B_j)
        print(f"A,B计算距离 = {res_L * 1000} m, 实际距离 = {case.L} m")
        print(f"A,B计算的相对方位角 = {res_bearing}，实际方位角 = {case.bearing}", end="\n\n")

        '''Ans: 已知A点经纬度，和A点到B点的距离和航向角，求B点经纬度'''
        res_lat,res_lon = getLonAndLat(A_j, A_w, bearing,L)
        print(f"B点计算经度 = {res_lon}, 实际经度 = {case.B_j}")
        print(f"B点计算纬度 = {res_lat}, 实际纬度 = {case.B_w}",end="\n\n")


    d = getDegree(40.14023,116.88945,40.12394,117.05897)
    print(d)